#include "osimsimulation.h"

namespace osimMethods
{
	// Constructor
	DECLDIR osimSimulation::osimSimulation()
	{
		
	}

	// Destructor
	DECLDIR osimSimulation::~osimSimulation()
	{

	}

	// Add empty model
	DECLDIR void osimSimulation::addModel(void)
	{
		//OpenSim::Model model();
		//osimModels[1].setName("modified_model.xml");
		//osimModels[1].copyData(model);
	}

	// Add model from file
	DECLDIR void osimSimulation::addModelfromFile(std::string fileName)
	{
		// Create temporary model and copy members to stored model
		OpenSim::Model model(fileName);
		osimModels[1].setName(model.getName());
		osimModels[1].copyData(model);
		
		// Initialize system
		SimTK::State& si = osimModels[1].initSystem();

		// Print info
		osimModels[1].printBasicInfo(std::cout);
		//osimModels[1].printDetailedInfo(si,std::cout);
	}

	// Get Model name
	DECLDIR std::string osimSimulation::getModelName(void)
	{
		std::string name = osimModels[1].getName();
		return name;
	}
	
	// Get tree items
	DECLDIR std::map<int,std::map<int,std::string>> osimSimulation::getTreeItems(void)
	{
		// Create a data structure
		std::map<int,std::map<int,std::string>> treeitems;
			std::map<int,std::string> bodyitems; // Bodies
			std::map<int,std::string> jointitems; // Joints
			std::map<int,std::string> constraintitems; // Constraints
			std::map<int,std::string> contactgeometryitems; // Contact geometry
			std::map<int,std::string> forceitems; // Forces
			std::map<int,std::string> markeritems; // Markers

		// Extract body names from model
		OpenSim::BodySet bodyset = osimModels[1].getBodySet();
		for (int i = 0; i < bodyset.getSize(); i++) { bodyitems[i] = bodyset[i].getName();
		if (bodyset[i].getDisplayer()->getNumGeometryFiles() > 0) { std::cout << bodyset[i].getDisplayer()->getGeometryFileName(0) << std::endl; } }
		treeitems[1] = bodyitems;

		// Extract joint names from model
		OpenSim::JointSet jointset = osimModels[1].getJointSet();
		for (int i = 0; i < jointset.getSize(); i++) { jointitems[i] = jointset[i].getName(); }
		treeitems[2] = jointitems;

		// Extract constraint names from model
		OpenSim::ConstraintSet constraintset = osimModels[1].getConstraintSet();
		for (int i = 0; i < constraintset.getSize(); i++) { constraintitems[i] = constraintset[i].getName(); }
		treeitems[3] = constraintitems;

		// Extract contact geometry names from model
		OpenSim::ContactGeometrySet contactgeometryset = osimModels[1].getContactGeometrySet();
		for (int i = 0; i < contactgeometryset.getSize(); i++) { contactgeometryitems[i] = contactgeometryset[i].getName(); }
		treeitems[4] = contactgeometryitems;

		// Extract force names from model
		OpenSim::ForceSet forceset = osimModels[1].getForceSet();
		for (int i = 0; i < forceset.getSize(); i++) { forceitems[i] = forceset[i].getName(); }
		treeitems[5] = forceitems;

		// Extract marker names from model
		OpenSim::MarkerSet markerset = osimModels[1].getMarkerSet();
		for (int i = 0; i < markerset.getSize(); i++) { markeritems[i] = markerset[i].getName(); }
		treeitems[6] = markeritems;

		return treeitems;
	}

}